/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file socketThread.cxx
	\brief class implemenation for internet socket thread receiving data

								    \author Jack Elston
						 $Date: 2005/02/12 00:20:45 $
*/
#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "socketThread.h"

	/*<-------Defines--------->*/
	/*<------End Defines------>*/

	/*<---Global Variables---->*/
		/*! \brief determines level of output to user. */
		extern bool verbose;
	/*<-End Global Variables-->*/

SocketThread::SocketThread(String name,
                           String port,
                           Socket::SocketType type,
                           Socket::SocketMode mode,
                           String host,
													 bool connect
                           ) : ThreadedPipe(name)
{

	sock = NULL;

	hostStr  = host;
	portStr  = port;
	sockType = type;
	sockMode = mode;

	connect_on_init = connect;
	if(!connect_on_init) trying_to_connect = false;
	else trying_to_connect = true;
	waiting_response = false;

	initSocket();
}

void SocketThread::initSocket() {
	if( sock != NULL ) {
		sock->close();
		delete sock;
		sock = NULL;
	}

	sock = new Socket(hostStr.c_str(), portStr.c_str(), sockType);
	if( sock == NULL ) {
		cout << getName() << "::init() - could not get new socket, " <<  strerror(errno) << endl;
	} else
		sock->setNonBlocking();
}

bool SocketThread::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	return true;
}

ThreadedPipe::commMode SocketThread::mode()
{
	switch(sockMode){
		case Socket::CLIENT:
		case Socket::LISTENING_CLIENT:
			return ThreadedPipe::TX;
			break;
		case Socket::SERVER:
		case Socket::RECEIVING_CLIENT:
			return ThreadedPipe::RX;
			break;
		default:
			return ThreadedPipe::DUPLEX;
			break;
	}
}

/************************************************************\
|                 SocketThread::init                         |
\************************************************************/
/*!
	\brief the initializing function for the internet side receiving thread.
	\param shm shared memory struct between the gateway threads.
	\param port port number of the socket connection.
	\param type type of socket, TCP or UDP.
	\param mode mode of thread, CLIENT or SERVER.
	\param host host for the connection.
	\pre instance of InetRx_Socket has been created
	\post socket and inetRxFifo have been intialized
	\return TRUE on successful completion of initialization, FALSE otherwise
 */
int SocketThread::readUDP()
{
	int n = sock->read((char *)fifo_msg.data, FIFO_BUF_SIZE);
	if(n<0) return n;
	else if (n > 0 ) {
		fifo_msg.src.s_addr = sock->getClientIP(-1).s_addr;
		//fifo_msg.dest.s_addr = sock->getServerIP().s_addr;
		Client * me = shmPtr->clients.getClient(0);
		//elstonj -- added this string to get rid of echoes in XML
		if((fifo_msg.src.s_addr) == ((shmPtr->clients.getAddr(me)).s_addr)) 
			return 0;
		fifo_msg.dest.s_addr= (shmPtr->clients.getAddr(me)).s_addr;
		if(verbose)
			cout << "SocketThread::readUDP - read " << (int)n << " bytes from " << inet_ntoa(fifo_msg.src) << endl;
	}

	return n;
}

int SocketThread::readTCP(fd_set readFDs)
{
//cout << "SocketThread::readTCP " << endl;
	Socket::SocketWait status = sock->checkFD(readFDs);
	if( sockMode != Socket::CLIENT && sockMode != Socket::DUPLEX_CLIENT && sockMode != Socket::RECEIVING_CLIENT) {
		for(int i = 0; i < sock->getNumClients(); i++) {
			if(status & 0x1 << i) {            // check if FD is active
				int n = sock->read(i,(char *)fifo_msg.data, FIFO_BUF_SIZE);
				if( n > 0) {                   // data available
					//fifo_msg.src.s_addr = sock->getClientIP(-1).s_addr;
					fifo_msg.src.s_addr = sock->getClientIP(i).s_addr;
					fifo_msg.dest.s_addr = sock->getServerIP().s_addr;
					/*cout << "--------->>> <<<---------" << endl;
					for(int z=0;z<n;z++)
						cout << (char)(fifo_msg.data[z]) << flush;
					cout << "--------->>> <<<---------" << endl;*/
//cout << "cory: client=" << i << " addr=" << inet_ntoa(fifo_msg.src) << endl;
          if(verbose)
            cout << getName() << "::readTCP() - read " << n << " bytes from socket" << endl;
					return n;
				} else {
					if( n == 0);
					else if( errno != EWOULDBLOCK || errno != EINTR) 
						cout << getName() << "::update() - bad read from client, " <<  strerror(errno) << endl;
					if(sock->removeClient(i) != i) {
						cout << getName() << "::update() - could not remove client" <<endl;
					} else {
						if(verbose)
							cout << getName() << "::update() - removed client" << endl;
					}
				}
			}
		}
	}
	else {
		int n = sock->read((char *)fifo_msg.data, FIFO_BUF_SIZE);
		if( n > 0) {                   // data available
//cout << "cory1" << endl;
			fifo_msg.dest.s_addr = sock->getClientIP(-1).s_addr;
			fifo_msg.src.s_addr = sock->getServerIP().s_addr;
			return n;
		} else {
			if( n == 0);
			else if( errno != EWOULDBLOCK || errno != EINTR) 
				cout << getName() << "::update() - bad read from client, " <<  strerror(errno) << endl;
		}
	}
	return 0;
}

int SocketThread::writeUDP(int num)
{
	if(! sock->isClosed() ) {
		int n = 0;
		// get our local broadcast adder for thenetwork
		Client * me = shmPtr->clients.getClient(0);
		in_addr bcast = shmPtr->clients.getBAddr(me);

		if(fifo_msg.dest.s_addr == bcast.s_addr && ((fifo_msg.dest.s_addr & inet_addr("255.0.0.0")) == inet_addr("6.0.0.0"))) {
			/*-----                     COMPUTERS                      -----*/
			fifo_msg.dest.s_addr = inet_addr("6.0.0.1");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.2");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.3");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			/*-----                      NexSTAR                       -----*/
			/*fifo_msg.dest.s_addr = inet_addr("6.0.0.32");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.33");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.34");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.35");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.36");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);*/
			/*-----                      Tempest                       -----*/
			fifo_msg.dest.s_addr = inet_addr("6.0.0.48");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.49");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.50");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			/*-----                        MNR                         -----*/
			fifo_msg.dest.s_addr = inet_addr("6.0.0.64");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.65");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.66");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			fifo_msg.dest.s_addr = inet_addr("6.0.0.67");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
			/*-----                       Other                        -----*/
			/*fifo_msg.dest.s_addr = inet_addr("6.0.0.128");
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);*/
		} else {
			n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
		}

		//n = sock->write(fifo_msg.dest,(char *)fifo_msg.data, num);
		if(num != n) {
			cout << getName() << "::update() - wrote " << n << " of " << fifo_msg.size 
			     << " bytes to socket" << endl;
		} else if(verbose) {
				cout << getName() << "::update() - wrote " << n << " bytes to " << inet_ntoa(fifo_msg.dest) << endl;
		}
		return n;
	}
	cout << "sock closed" << endl;
	return 0;
}

int SocketThread::writeTCP(fd_set writeFDs, int num)
{
	int n = 0;
	Socket::SocketWait status = sock->checkFD(writeFDs);
	for(int i = 0; i < sock->getNumClients(); i++) {
		if(status & (0x1 << i)) { // check if FD is active 
			n = sock->write(i,(char *)fifo_msg.data, num);
			if( num == n) {                    // wrote data
        if(verbose)
          cout << getName() << "::update() - wrote " << n << " bytes to socket" << endl;
			} else {
				if( n == 0)
					cout << getName() << "::update() - write to client returned zero bytes: errno=" 
					     << strerror(errno) << endl;
				else if( errno != EWOULDBLOCK || errno != EINTR) 
					cout << getName() << "::update() - bad write to client, " <<  strerror(errno) << endl;

				if(sock->removeClient(i) != i) {
					cout << getName() << "::update() - could not remove client" <<endl;
				} else {
					if(verbose)
						cout << getName() << "::update() - removed client" << endl;
				}
			}
		}
	}
	if(sockMode == Socket::DUPLEX_CLIENT || sockMode == Socket::CLIENT) {
		n = sock->write(0,(char *)fifo_msg.data, num);
		if( num != n) {
			if( errno != EWOULDBLOCK || errno != EINTR) {
				cout << getName() << "::update() - bad write to client, " <<  strerror(errno) << endl;
        sock->close();
			}
		}
	}
	if(verbose)
		cout << getName() << "::update() - wrote " << n << " bytes to socket" << endl;
	return n;
}

bool SocketThread::connectSocket() {
		if( sock->isClosed() ) {
			if ( !sock->open( sockMode ) ) {
				cout << getName() << "::update() - Could not open sock" << endl; 

				// timeout for a second and try again 
				sleep(1); 
				return false;
			} 
		} 

		if(sockMode == Socket::CLIENT || sockMode == Socket::DUPLEX_CLIENT || sockMode== Socket::RECEIVING_CLIENT) {
			if(!sock->isConnected()) {
				if(verbose)
					cout << getName() << "Trying to connect ... " << flush;
				sock->connectHost();
			}

			// If we did not connect, timeout for a second and then try again 
			if(!sock->isConnected()) {
				if(verbose)
					cout << "                          [ FAIL ]" << endl;
				sleep(1); 
				return false;
			}
		}
		return true;
}


/************************************************************\
|                 SocketThread::update                       |
\************************************************************/
/*!
	\brief function called periodically by the thread.
	\pre instance of InetRx_Socket has been created and initialized
	\post connection list is refreshed, new data is placed in inetRxFifo
 */
void SocketThread::update() 
{
	int maxFD, val, n=0;
	fd_set readFDs, writeFDs;
	Socket::SocketWait status;
	struct timeval timeout;

	/*<---Check Socket Conn--->*/
	if ( trying_to_connect ) {
		if( !connect_on_init ) {
			if(connectSocket()) {
				if(waiting_response) {
					CMD_CMD(fifo_msg.data)  = CMD_CONNECT;
					ACK(INBOARD_TX);
					waiting_response = false;
				}
			} else {
				CMD_CMD(fifo_msg.data)  = CMD_CONNECT;
				trying_to_connect = false;
				NACK(INBOARD_TX);
				waiting_response = false;
			}
		} else {
			connectSocket();
		}
	}

	/*<------Select FDs------->*/
	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	if(trying_to_connect) {
		maxFD = sock->setFD(readFDs);
		if(sockMode == Socket::LISTENING_CLIENT || sockMode == Socket::CLIENT || sockMode == Socket::DUPLEX || sockMode == Socket::DUPLEX_CLIENT || sockMode == Socket::DUPLEX_SERVER ) {
			FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
			sock->setFD(writeFDs,TRUE);
			maxFD = (fifo_FDs[INBOARD_RX] > maxFD ? fifo_FDs[INBOARD_RX] : maxFD);
		}
		if(sockMode == Socket::SERVER || sockMode == Socket::DUPLEX || sockMode == Socket::DUPLEX_CLIENT || sockMode == Socket::DUPLEX_SERVER || sockMode == Socket::RECEIVING_CLIENT) {
			FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
			maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);
		}
	} else {
		FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
		FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
		maxFD = (fifo_FDs[INBOARD_TX] > fifo_FDs[INBOARD_RX] ? fifo_FDs[INBOARD_TX] : fifo_FDs[INBOARD_RX]);
	}

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
		;
	}
	/*<----One FD is Active--->*/
	else {

		if(!trying_to_connect) {
			if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))  {
				if( (n = readFifo(INBOARD_RX)) ) {
					Command cmd((uint8_t*)fifo_msg.data);
					if(cmd.isValid()) 
						parse(cmd, fifo_msg.src);
				}
			}
			return;
		}

		status = sock->checkFD(readFDs);

		bool new_fifo_data = (fifo_FDs[INBOARD_RX] > 0) && (FD_ISSET(fifo_FDs[INBOARD_RX], & readFDs));
		if((status & ~Socket::WAIT_PEER) > 0 || new_fifo_data || sockType == Socket::UDP || (status && sockMode == Socket:: DUPLEX_CLIENT && sock->isConnected()) ||  (status && sockMode == Socket:: RECEIVING_CLIENT && sock->isConnected()) ) {

			/*<---Server Operations--->*/
			if(sockMode == Socket::SERVER || sockMode == Socket::DUPLEX || sockMode == Socket::DUPLEX_CLIENT || sockMode == Socket::DUPLEX_SERVER || sockMode == Socket::RECEIVING_CLIENT) {
				if(sockType == Socket::UDP)
					n = readUDP();
				else if(sockType == Socket::TCP)
          if((status & ~Socket::WAIT_DATA) > 0) {
            n = readTCP(readFDs);

            if(verbose)
              cout << "SocketThread::update() - read " << (int)n << " bytes" << endl;
            if(n == 0) sock->close();
          }

				if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
					if(n > 0) {
//cout << "SocketThread::update() - sending " << (int)n << " bytes to INBOARD_TX" << endl;
						fifo_msg.size = n;
						writeFifo(INBOARD_TX);
					}
				}
			}

			/*<---Client Operations--->*/
			if(sockMode == Socket::CLIENT || sockMode == Socket::LISTENING_CLIENT 
					|| sockMode == Socket::DUPLEX || sockMode == Socket::DUPLEX_CLIENT || sockMode == Socket::DUPLEX_SERVER) {
				if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))  {
					if( (n = readFifo(INBOARD_RX)) ) {
//cout << "SocketThread::update() - read " << (int)n << " bytes on INBOARD_RX" << endl;
//cout << "SocketThread::update() - sending " << (int)n << " bytes to local write" << endl;
						uint16_t length = CMD_SIZE(fifo_msg.data);
						if( length == fifo_msg.size && length < 1000) {
							uint8_t id = CMD_CMD(fifo_msg.data);
							if(id == CMD_COMM || id == CMD_ACK || id == CMD_FAIL) {
								Command cmd((unsigned char *)fifo_msg.data);
								if(cmd.isValid() && cmd.cmd() == CMD_CONNECT) {
									parse(cmd, fifo_msg.src);
									return;
								}
							}
						}

						if(sockType == Socket::UDP)
							writeUDP(n);
						else if(sockType == Socket::TCP) 
							writeTCP(writeFDs, n);
						
					} else {
						if( n == 0)
							cout << getName() << "::update() - read zero bytes on INBOARD_RX, "
									 << strerror(errno) << endl;
						else if( errno != EWOULDBLOCK || errno != EINTR) 
							cout << getName() << "::update() - bad reaon on INBOARD_RX, " <<  strerror(errno) << endl;
					}
				}
			}
		}
		/*<---Client is Waiting--->*/
		if(sockMode != Socket::CLIENT && sockMode != Socket::DUPLEX_CLIENT && sockMode != Socket::RECEIVING_CLIENT) {
			if(status & Socket::WAIT_PEER) {
				sock->connectClient();
				//int client = sock->connectClient();
				//if(sockType == Socket::UDP) {
					//if(sock->getClientIP(client).s_addr != (inet_makeaddr(127,1)).s_addr)
						//shmPtr->clients.add(sock->getClientIP(client));
				//}
			}
		}
	}
}

/************************************************************\
|                 SocketThread::close                        |
\************************************************************/
/*!
	\brief closes socket connection and inetRxFifo
	\pre instance of InetRx_Socket has been created and initialized
	\post socket connections have been closed and inetRxFifo has been closed and deleted 
 */
void SocketThread::close() 
{
	ThreadedPipe::close();
	if( sock != NULL) {

		sock->close();

		if(verbose)
			sock->printStats();

		delete sock;
		sock = NULL;
	}
}

void SocketThread::parse(const Command &cmd, in_addr address) 
{
	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

	switch(cmd.cmd()) {

		case CMD_CONNECT: {
			CommStruct_t * request = (CommStruct_t *)dataPtr;

			if(request->connect && !connect_on_init) {
				trying_to_connect = true;
				waiting_response = true;
				hostStr  = inet_ntoa(request->ip);
				char port_str[32];
				sprintf(port_str,"%d",request->port);
				portStr  = port_str;

				initSocket();
			} else {
				if(!(request->connect) && !connect_on_init) {
					trying_to_connect = false;
					waiting_response = false;
					sock->close();
					Client * me = shmPtr->clients.getClient(0);
					fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
					ACK(INBOARD_TX);
				}
			}
	  } break;
	} // end of command switch
}
